#ifndef LOCAL_PLANNER_UTILS_H
#define LOCAL_PLANNER_UTILS_H
#include<sys/time.h>
#include <iostream>
#include <cmath>
#include <fstream>
#include <vector>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <string.h>
#include <sstream>
#include <iomanip>


namespace vehicle{
namespace tools{


#define distance2points(from, to) sqrt(pow(to.x_ - from.x_, 2) + pow(to.y_ - from.y_, 2))
#define distance2points_pow(from, to) pow(to.x_ - from.x_, 2) + pow(to.y_ - from.y_, 2)
#define calLength(v) sqrt(v.x_ *v.x_ + v.y_ * v.y_)
#define DEG2RAD M_PI / 180.
#define RAD2DEG 180. / M_PI

double calDiffBetweenTwoAngle(const double &a1, const double &a2);

double cast_from_0_to_2PI_Angle(const double &ang);

double cast_from_PI_to_PI_Angle(const double &ang);

double diffBetweenTwoAngle(const double &a1, const double &a2);

int64_t getCurrentTime();

bool fileExists(const std::string& filename);

std::vector<std::string> ListSubPaths(const std::string &directory_path,
                                      const unsigned char d_type);

bool RemoveAllFiles(const std::string &directory_path);

bool EnsureDirectory(const std::string &directory_path);
bool DirectoryExists(const std::string &directory_path);

void GetTime(std::string *date_time);


std::string formatFloat(double value, int precision);
} // namespace UtilityNS
}
#endif //LOCAL_PLANNER_UTILS_H
